#ifndef BLOCKINTERFACE_H
#define BLOCKINTERFACE_H

#include <Eigen/Core>
#include <limits>

namespace Estimator
{
// abstract class for define the interface for Parameter Block
/// @tparam Dim     Dimension of parameter block
/// @tparam MinDim  Minimal dimension of parameter block
/// @tparam T       The type of the estimate
template <size_t Dim, size_t LocalDim, class T>
class BlockBase
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    BlockBase() noexcept = default;
    BlockBase(size_t id) noexcept : mId_(id) {} 
    virtual ~BlockBase() = default;

    static constexpr size_t Dimension = Dim;
    static constexpr size_t LocalDimension = LocalDim;
    typedef T parameter_t;

    // method ..
    virtual void setEstimate(const parameter_t& estimate) = 0;
    virtual parameter_t getEstimate() const = 0;
    virtual std::string typeInfo() const = 0; // get type information for debug and assert

    // should always return the Pointer to the parameters allocated in here.
    double* parameters() { return mParameters_; } 

    const double* parameters() const { return mParameters_; }

    // get the parameter dimension
    size_t dimension() const{ return Dimension; }

    // get local parameter dimension.
    size_t localDimension() const{ return LocalDimension;}

    size_t GetId() const{ return mId_;}

  protected:
    double mParameters_[Dim];
    size_t mId_ = std::numeric_limits<size_t>::max();
};

// Abstract class for Factor Interface, or Edge type
/// @tparam ResidualDim     Dimension of residual cost funciton.
template <size_t ResidualDim = 3>
class InterfaceFactor
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef Eigen::Matrix<double, ResidualDim, ResidualDim> covariance_t;
    typedef Eigen::Matrix<double, ResidualDim, 1> measurement_t;

    InterfaceFactor(const covariance_t& information, const measurement_t& measurement) noexcept :  mMeasurement_(measurement), mInformation_(information) {}

    InterfaceFactor() = delete;

    virtual ~InterfaceFactor() = default;
    // returns the number of parameter blocks that connects to..
    virtual size_t parameterBlocks() const = 0;

    // API that set Information and covariance matrix..
    void setInformation(const covariance_t &information)
    {
      mInformation_ = information;
      mCovariance_ = information.inverse();

      Eigen::LLT<Eigen::Matrix<double, ResidualDim, ResidualDim>> lltOfInformation(mInformation_);
      mSquareRootInformation_ = lltOfInformation.matrixL().transpose();
    }

    void setMeasurement(const measurement_t &measurement)
    {
      mMeasurement_ = measurement;
    }

    const measurement_t &getMeasurement() const
    {
      return mMeasurement_;
    }

    measurement_t &getMeasurement()
    {
      return mMeasurement_;
    }
    
  protected:
    measurement_t mMeasurement_;
    covariance_t mInformation_;
    covariance_t mCovariance_;
    covariance_t mSquareRootInformation_;
};
}

#endif //BLOCKINTERFACE_H